#pragma once
#define PI_OVER_180 0.0174532925
#define SHUFFLE_PARAM(x,y,z,w) (x|(y<<2)|(z<<4)|(w<<6))
#define MM_CLONE_X(p) _mm_shuffle_ps(p,p,SHUFFLE_PARAM(0,0,0,0))
#define MM_CLONE_Y(p) _mm_shuffle_ps(p,p,SHUFFLE_PARAM(1,1,1,1))
#define MM_CLONE_Z(p) _mm_shuffle_ps(p,p,SHUFFLE_PARAM(3,2,2,2))
#define MM_CLONE_W(p) _mm_shuffle_ps(p,p,SHUFFLE_PARAM(3,3,3,3))
#define MM_MULTIPLY_AND_ADD(a,b,c) _mm_add_ps(_mm_mul_ps(a,b),c)

//Accelerated Math...a little TOO acellerated for me.

//based on my understanding, __m128 is a 4-float data structure that allows for Single Instruction Multiple Data calls, which makes
// repetive math good-fast. BUT it costs frames to transfer to and from floats, so its better to avoid that at the expense of memory.

struct Vec4{
__declspec(align(16)) float v[4]; 
Vec4(){v[0]=v[1]=v[2]=v[3]=0;}
Vec4(float x,float y, float z,float w){v[0]=x;v[1]=y;v[2]=z;v[3]=w;}
__m128 toMagicData(){return _mm_load_ps(&v[0]);}
};

struct Vec3{ //12 Bytes
__declspec(align(16)) float v[3]; //ya never know
Vec3(){v[0]=v[1]=v[2]=0;}
Vec3(float x, float y, float z){v[0]=x;v[1]=y;v[2]=z;}
Vec3 Cross(Vec3 *a){
Normalize(); a->Normalize();
Vec3 ret((v[1]*v[2]) -(a->v[2]*a->v[1]),
		 (v[2]*v[0]) -(a->v[0]*a->v[2]),
		 (v[0]*v[1]) -(a->v[1]*a->v[0]));
return ret;
}
float Dot(Vec3 *a){
Normalize(); a->Normalize();
float ret = (v[0]*a->v[0])+(v[1]*a->v[1])+(v[2]*a->v[2]);
return ret;
}
void Normalize(){
float W = pow(v[0],2)+pow(v[1],2)+pow(v[1],2);W=sqrt(W);
v[0]/=W;v[1]/=W;v[2]/=W;
}
Vec3 Normalized(){
float W = pow(v[0],2)+pow(v[1],2)+pow(v[1],2);W=sqrt(W);
return Vec3(v[0]/W,v[1]/W,v[2]/W);
}
Vec4 toVec4(){return Vec4(v[0],v[1],v[3],0);}
}; 

struct Mat4x4{ //64 Bytes
__m128 Rows[4];

Mat4x4(){
	LoadIdentity();
}

Mat4x4(float *mat){
Rows[0]=_mm_load_ps(&mat[0]);
Rows[1]=_mm_load_ps(&mat[4]);
Rows[2]=_mm_load_ps(&mat[8]);
Rows[3]=_mm_load_ps(&mat[12]);
}

void LoadIdentity(){
	__declspec(align(16)) float mat[4] = {1,0,0,0};
	Rows[0]=_mm_load_ps(mat);
	Rows[1] =_mm_shuffle_ps(Rows[0],Rows[0],SHUFFLE_PARAM(2,0,1,1));
	Rows[2] =_mm_shuffle_ps(Rows[0],Rows[0],SHUFFLE_PARAM(1,1,0,1));
	Rows[3] =_mm_shuffle_ps(Rows[0],Rows[0],SHUFFLE_PARAM(2,1,1,0));
}

void Translate(float tx,float ty,float tz){
	__declspec(align(16)) float trn[16] = {1,0,0,tx,
										   0,1,0,ty,
										   0,0,1,tz,
										   0,0,0,1};
	Mat4x4 b(trn);
	Multiplyby(&b);
}

void Rotate(float rx,float ry,float rz){
	float st[6] = {sinf(rx*PI_OVER_180),sinf(ry*PI_OVER_180),sinf(rz*PI_OVER_180),cosf(rx*PI_OVER_180),cosf(ry*PI_OVER_180),cosf(rz*PI_OVER_180)};
	/*cos y cos z	      - cos x sin z + sin x sin y cos z         sin x sin z + cos x sin y cos z
      cos y sin z           cos x cos z + sin x sin y sin z	       -sin x cos z + cos x sin y sin z
      -sin y		     	sin x cos y							    cos x cos y
*/
	__declspec(align(16)) float rot[16] = {st[4]*st[5],		-st[3]*st[2]+st[0]*st[1]*st[5]		,st[0]*st[2]+st[3]*st[1]*st[5], 0,//
										   st[4]*st[2],		 st[3]*st[5]+st[0]*st[1]*st[2]	   ,-st[0]*st[5]+st[3]*st[1]*st[2],0,//
										   -st[1],           st[0]*st[4]						,st[2]*st[4],					0,//
										                                                                                  0,0,0,1};// Mental exercise!
	Mat4x4 b(rot);
	Multiplyby(&b); //this seems expensive
}

void Scale(float sx,float sy,float sz){
	__declspec(align(16)) float scl[16] = {sx,0,0,0,
										   0,sy,0,0,
										   0,0,sz,0,
										   0,0,0,1};
	Mat4x4 b(scl);
	Multiplyby(&b);
}

void operator =(Mat4x4 *b){
Rows[0] = b->Rows[0];
Rows[1] = b->Rows[1];
Rows[2] = b->Rows[2];
Rows[3] = b->Rows[3];
}

void Ortho(float r,float l, float t, float b, float d, float n){
float tx = -((r+l)/(r-l)), ty = -((t+b)/(t-b)), tz = -((d+n)/(d-n));
__declspec(align(16)) float Mat[16] = {(2/(r-l)),0,0,tx,
										 0,(2/(t-b)),0,ty,
										 0,0,(2/(d-n)),tz,
										 0,0,0,1};
Mat4x4 ortomat = Mat4x4(Mat);
Multiplyby(&ortomat);
}

__m128 MultiplyVectorFast(__m128 v){
__m128 Result;
Result =         _mm_mul_ps(MM_CLONE_X(v),Rows[0]);
Result =MM_MULTIPLY_AND_ADD(MM_CLONE_Y(v),Rows[1],Result);
Result =MM_MULTIPLY_AND_ADD(MM_CLONE_Z(v),Rows[2],Result);
Result =MM_MULTIPLY_AND_ADD(MM_CLONE_W(v),Rows[3],Result);
return Result;
}

void GetMatrix(float *mat){
_mm_storeu_ps (&mat[0], Rows[0]);
_mm_storeu_ps (&mat[4], Rows[1]);
_mm_storeu_ps (&mat[8], Rows[2]);
_mm_storeu_ps (&mat[12], Rows[3]); //fill them with the matrix;
}

Mat4x4 MultiplyMatrix(Mat4x4 *b){
Mat4x4 Result;
for(int i=0; i<4; i++){
Result.Rows[i] =         _mm_mul_ps(MM_CLONE_X(Rows[i]),b->Rows[0]);
Result.Rows[i] =MM_MULTIPLY_AND_ADD(MM_CLONE_Y(Rows[i]),b->Rows[1],Result.Rows[i]);
Result.Rows[i] =MM_MULTIPLY_AND_ADD(MM_CLONE_Z(Rows[i]),b->Rows[2],Result.Rows[i]);
Result.Rows[i] =MM_MULTIPLY_AND_ADD(MM_CLONE_W(Rows[i]),b->Rows[3],Result.Rows[i]);
}
return Result;
}

void Multiplyby(Mat4x4 *b){
__m128 Result[4];
for(int i=0; i<4; i++){
Result[i] =         _mm_mul_ps(MM_CLONE_X(Rows[i]),b->Rows[0]);
Result[i] =MM_MULTIPLY_AND_ADD(MM_CLONE_Y(Rows[i]),b->Rows[1],Result[i]);
Result[i] =MM_MULTIPLY_AND_ADD(MM_CLONE_Z(Rows[i]),b->Rows[2],Result[i]);
Result[i] =MM_MULTIPLY_AND_ADD(MM_CLONE_W(Rows[i]),b->Rows[3],Result[i]);
}
Rows[0] = Result[0];
Rows[1] = Result[1];
Rows[2] = Result[2];
Rows[3] = Result[3];
}

};


struct Plane{ //16 Bytes
float x,y,z,d;

};

struct Location{//24 bytes
Vec3 Position   //12
float Rotation; //4
float WScale;   //4
float HScale;   //4

Mat4x4 toMatrix(){
Mat4x4 ret;
ret = Mat4x4();
ret.Translate(Position.v[0],Position.v[1],Position.v[2]);
ret.Rotate(0,0,Rotation);
ret.Scale(WScale,HScale,1.0);
return ret;
}

};






